# Copyright 2021 Josh Newans
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import rclpy
from rclpy.node import Node

from std_msgs.msg import Header
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TwistWithCovariance
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseWithCovariance
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TwistStamped
from geometry_msgs.msg import PoseStamped
from rclpy.qos import QoSProfile, ReliabilityPolicy
class TwistStamper(Node):

    def __init__(self):
        super().__init__('odom')#节点名称

        self.declare_parameter("child_link", "")# 声明一个名为'child_link'的参数，默认值为空字符串，参数可用于在运行时配置节点的某些特性
        self.child_link = str(self.get_parameter("child_link").value)# 获取刚刚声明的'child_link'参数的值，并转换为字符串类型，赋值给self.frame_id变量

        self.publisher_odom = self.create_publisher(Odometry, 'odom', 100)#创建发布方
        # self.timer = self.create_timer(2.0,self.on_timer)

        # 设置为不可靠（BEST_EFFORT）的QoS策略
        qos_profile = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT  # 设置为不可靠性
        )

        #创建订阅方3个，订阅odom的header,posewithcov和twistwithcov
        self.ap_pose_sub = self.create_subscription(
            PoseStamped,
            'ap/pose/filtered',
            self.listener_callback_ap_pose_sub,
            qos_profile)
        self.pose_cov_sub = self.create_subscription(
            PoseWithCovariance,
            'posewithcovariance',
            self.listener_callback_pose_cov_sub,
            10)
        self.twist_cov_sub = self.create_subscription(
            TwistWithCovariance,
            'twistwithcovariance',
            self.listener_callback_twist_cov_sub,
            10)
        # self.subscription  # prevent unused variable warning
        # print("hello!pub_sub")

        self.posewithcovariance = None
        self.twistwithcovariance = None
        self.posestampedheader = None
        # print("hello!None")
        self.get_logger().info("等待posewithcovariance and twistwithcovariance ...") 

    #订阅方回调函数，订阅到数据的话就执行此函数
    def listener_callback_pose_cov_sub(self, inMsg):
        self.posewithcovariance = inMsg
        self.odom_publish()
        # print("hello!0")
        # self.timer = self.create_timer(2.0,self.on_timer)
        # self.publisher()
    def listener_callback_twist_cov_sub(self, inMsg1):
        self.twistwithcovariance = inMsg1
        self.odom_publish()
        # print("hello!1")
        # self.timer = self.create_timer(2.0,self.on_timer)
        # self.publisher()
    def listener_callback_ap_pose_sub(self, inMsg2):
        self.posestampedheader = inMsg2.header
        self.odom_publish()
        # self.timer = self.create_timer(2.0,self.on_timer)
        # print("hello!2")

    def odom_publish(self):
        odom = Odometry()
        odom.child_frame_id = self.child_link
        # odom.header = self.posestampedheader
        #这里由于TF坐标变换先注释了
        odom.header = self.posestampedheader if self.posestampedheader else Header()
        
        # odom.pose = self.posewithcovariance
        # odom.twist = self.twistwithcovariance
        #用TF坐标变换来解决NED-ENU问题,不行
        #1.这是NED坐标系下的odom，叫做odom_ned,他有一条自己的TF链，odom_ned->odom
        # odom.header.frame_id = "odom_ned";  # 原始数据基于NED坐标系
        # odom.header.stamp = Header().stamp
        #------------------------------------
        if self.posewithcovariance is not None:
            odom.pose = self.posewithcovariance  # Set the pose
            # odom.pose.covariance = self.posewithcovariance  # Set the covariance
        else:
            self.get_logger().warn("PoseWithCovariance is not set")

        if self.twistwithcovariance is not None:
            odom.twist = self.twistwithcovariance  # Set the twist
            # odom.twist.covariance = self.twistwithcovariance.covariance  # Set the covariance
        else:
            self.get_logger().warn("TwistWithCovariance is not set")


        self.publisher_odom.publish(odom)
        # self.get_logger().info('X: "%f"' % outMsg.twist.linear.x)
        self.get_logger().info(f'odom  position pose x: {odom.pose.pose.position.x} \n child_frame_id:{odom.child_frame_id}')
        # self.get_logger().info("发布pose：x:%d " % (odom.pose))
        # print("hello!odom")

def main(args=None):
    rclpy.init(args=args)

    twist_stamper = TwistStamper()
    rclpy.spin(twist_stamper)

    twist_stamper.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
